 /*
ArduPilot Version 2.8 By Jason Short
*/

// ARDUPILOT 2.8
// April 30, 2010
// Hardware: Ardupilot (not mega)

// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_RC.h>			// ArduPilot GPS library
#include <AP_RC_Channel.h> 	// ArduPilot RC Library
#include <AP_GPS.h>			// ArduPilot GPS library
#include <AP_PID.h>         // ArduPilot PID library

// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <math.h>

// Local modules
#include "defines.h"

#include "AP_Config.h"
#include "AP_PID_settings.h"

////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial);		// FTDI/console

// All GPS access should be through this pointer.
GPS         *g_gps;


#if GPS_PROTOCOL == GPS_PROTOCOL_SIRF
	AP_GPS_SIRF     g_gps_driver(&Serial);

#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
	AP_GPS_UBLOX    g_gps_driver(&Serial);

#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
	AP_GPS_MTK      g_gps_driver(&Serial);

#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
	AP_GPS_MTK16    g_gps_driver(&Serial);

#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
	AP_GPS_None     g_gps_driver(NULL);

#else
	#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL



// GENERAL VARIABLE DECLARATIONS
// --------------------------------------------
byte 	control_mode		= MANUAL;
byte	oldSwitchPosition;

const char* flight_mode_strings[] = {
	"Manual",
	"Circle",
	"Stabilize",
	"FBW_A",
	"FBW_B",
	"Config",
	"Auto",
	"RTL",
	"Loiter",
	"Takeoff",
	"Land"};

// Failsafe
// --------
boolean failsafe			= false;	// did our throttle dip below the failsafe value?
boolean ch3_failsafe		= false;
byte 	crash_timer			= 0;
bool	throttle_fs_enabled;
//int 	egg_dist 			= 0;

// RC channels
AP_RC rc;
AP_RC_Channel rc_1(EE_RADIO_1);
AP_RC_Channel rc_2(EE_RADIO_2);
AP_RC_Channel rc_3(EE_RADIO_3);
AP_RC_Channel rc_4(EE_RADIO_4);

#if MIXING_MODE == 0
	uint16_t elevon1_trim	= 1500; 	// TODO: handle in EEProm
	uint16_t elevon2_trim	= 1500;
	uint16_t ch1_temp		= 1500;     // Used for elevon mixing
	uint16_t ch2_temp		= 1500;

	// TODO: switch these reverses to true/false, after they are handled by AP_RC_Channel
	int 	reverse_elevons = 1;
	int 	reverse_ch1_elevon = 1;
	int 	reverse_ch2_elevon = 1;
#endif

// PID controllers
AP_PID         pidServoRoll;
AP_PID         pidServoPitch;
AP_PID         pidNavRoll;
AP_PID         pidNavPitchAirspeed;
AP_PID         pidNavPitchAltitude;
AP_PID         pidTeThrottle;

float 	nav_gain_scaler	= 1;			// Gain scaling for headwind/tailwind

// LED output
// ----------
boolean GPS_light;							// status of the GPS light

// GPS variables
// -------------
const 	float t7			= 10000000.0;	// used to scale GPS values for EEPROM storage
float 	scaleLongUp			= 1;			// used to reverse longtitude scaling
float 	scaleLongDown		= 1;			// used to reverse longtitude scaling
byte 	ground_start_count	= 10;			// have we achieved first lock and set Home?
bool	GPS_enabled 		= false;		// used to quit "looking" for gps with auto-detect if none present

// Location & Navigation
// ---------------------
byte 	wp_radius			= 20;			// meters
long	nav_bearing;						// deg * 100 : 0 to 360 current desired bearing to navigate
long 	target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
long 	old_target_bearing;					// deg * 100
long 	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of plane to target
int 	climb_rate;							// m/s * 100
long 	hold_course 		= -1;			// deg * 100 dir of plane
int		alt_to_hold;						// how high we should be for RTL
byte	loiter_radius 		= LOITER_RADIUS; // meters


// Airspeed
// --------
float 	airspeed 			= 0; 							// m/s * 100
int		airspeed_cruise		= AIRSPEED_CRUISE * 100;		// m/s * 100 : target airspeed sensor value
int     airspeed_nudge;  					// m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
float   airspeed_error;						// m/s * 100
long    energy_error;                       // energy state error (kinetic + potential) for altitude hold
long    airspeed_energy_error;              // kinetic portion of energy error
boolean airspeed_enabled;

int		airspeed_test;
boolean airspeed_flag;
int		throttle_test;
int		airpseed_test;
float	speed_scaler;

// Location Errors
// ---------------
long 	bearing_error; 						// deg * 100 : 18000 to -18000
long 	altitude_error;						// meters * 100 we are off in altitude
float	crosstrack_error;					// deg * 100 : 18000 to -18000  meters we are off trackline

float 	crosstrack_gain;
int 	crosstrack_entry_angle;

// Sensors
// --------
float 	airpressure_raw		= 511;			// Airspeed Sensor - is a float to better handle filtering
int 	airpressure_offset;					// analog air pressure sensor while still
int 	airpressure;						// airspeed as a pressure value
float 	battery_voltage 	= 1024;			// Battery Voltage
float 	filter_batt_voltage = 511; 			// Filtered Battery Voltage mjc

int 	analog0				= 511;			// Thermopiles - Pitch
int 	analog1				= 511;			// Thermopiles - Roll
int 	analog2				= 511;			// Thermopiles - Z
int 	ir_max				= 300;			// used to scale Thermopile output to 511
int 	ir_max_save			= 300;			// used to scale Thermopile output to 511

// From IMU - I wish
long 	roll_sensor;						// how much we're turning in deg * 100
long 	pitch_sensor;						// our angle of attack in deg * 100


// flight mode specific
// --------------------
boolean takeoff_complete	= false;		// Flag for using take-off controls
boolean land_complete		= 0;

// Loiter management
// -----------------
long 	saved_target_bearing;				// deg * 100
int		loiter_total;						// deg : how many times to loiter * 360
int 	loiter_delta;						// deg : how far we just turned
int		loiter_sum;							// deg : how far we have turned around a waypoint
long 	loiter_time;						// millis : when we started LOITER mode
long 	loiter_time_max;					// millis : how long to stay in LOITER mode


// these are the values for navigation control functions
// ----------------------------------------------------
long 	nav_roll;							// deg * 100 : target roll angle
long 	nav_pitch;							// deg * 100 : target pitch angle
long 	nav_yaw;							// deg * 100 : target pitch angle
int     throttle_nudge;                 	// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
int 	throttle_cruise	= THROTTLE_CRUISE;	// 0-100 : target throttle output for average speed
long	altitude_estimate;					// for smoothing GPS output

bool imu_ok;

// Waypoints
// ---------
long 	wp_distance;						// meters - distance between plane and next waypoint
long 	wp_totalDistance;					// meters - distance between old and next waypoint
byte 	wp_total;							// # of waypoints
byte 	wp_index;							// Current WP index, -1 is RTL


// 3D Location vectors
// -------------------
struct 	Location home;						// home location
struct 	Location prev_WP;					// last waypoint
struct 	Location current_loc;				// current location
struct 	Location next_WP;					// next waypoint
long 	target_altitude;					// used for altitude management between waypoints
long 	offset_altitude;					// used for altitude management between waypoints
boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the home location


// System Timers
// --------------
unsigned long 	fast_loopTimer;				// Time in miliseconds of main control loop
unsigned long 	medium_loopTimer;			// Time in miliseconds of navigation control loop
byte 			medium_loopCounter;			// Counters for branching from main control loop to slower loops
byte 			slow_loopCounter;			//
uint8_t 		delta_ms_fast_loop; 		// Delta Time in miliseconds
unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
int 			mainLoop_count;
unsigned long 	elapsedTime;				// for doing custom events

////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////

// Basic Initialization
//---------------------
void setup() {
	init_ardupilot();
}

void loop()
{
	// We want this to execute at 50Hz if possible
	// -------------------------------------------
	if (DIYmillis()-fast_loopTimer > 19) {
		delta_ms_fast_loop 	= DIYmillis() - fast_loopTimer;
		fast_loopTimer		= DIYmillis();

		// Execute the fast loop
		// ---------------------
		fast_loop();

		// Execute the medium loop
		// -----------------------
		medium_loop();

		mainLoop_event();
	}
}

void fast_loop()
{
	// Read radio
	// ----------
	read_radio();
	//print_servo_out();

	// check for throtle failsafe condition
	// ------------------------------------
	#if THROTTLE_FAILSAFE == 1
		set_failsafe(ch3_failsafe);
	#endif

	// Read Airspeed
	// -------------
	#if AIRSPEED_SENSOR == 1 && GPS_PROTOCOL != GPS_PROTOCOL_IMU  // 5 - sim
		read_airspeed();
	#endif

	// read in the plane's attitude
	// ----------------------------
	#if GPS_PROTOCOL == GPS_PROTOCOL_IMU
		// We are using the IMU
		// ---------------------
		decode_gps();

	#elif GPS_PROTOCOL == 99
		// this is the sim
		// ---------------
		read_sim_sensors();

	#else
		// 	We are using FMA sensors
		// ------------------------
		read_XY_sensors();

		// if we are using a Z sensor, read that in too
		// --------------------------------------------
		#if ENABLE_Z_SENSOR == 1
		read_z_sensor();
		#endif

	#endif

	// altitude smoothing
	// ------------------
	calc_altitude_error();

	// How off is our heading?
	// -----------------------
	calc_bearing_error();

	//print_servo_out();

	// custom code/exceptions for flight modes
	// ---------------------------------------
	update_current_flight_mode();

	// apply desired roll, pitch and yaw to the plane
	// ----------------------------------------------
	if (control_mode > MANUAL){
		calc_speed_scaler();
		stabilize_pitch();
		stabilize_roll();

		#if CH4_RUDDER == 1
			stabilize_yaw();
		#endif
	}
	//roll: 0	ci:-4456	so:-3564	ro:1851

	//print_servo_out();

	// write out the servo PWM values
	// ------------------------------

	if(control_mode == MANUAL){
		output_manual();
	}
	set_servos_4();

	#if GCS_PROTOCOL == 3
		output_Xplane();
	#endif


	#if GPS_PROTOCOL == GPS_PROTOCOL_IMU
		if (mainLoop_count != 0){
			//printPerfData();
			send_message(MSG_PERF_REPORT);
		}
	#endif
}

void medium_loop()
{
	// This is the start of the medium (10 Hz) loop pieces
	// -----------------------------------------
	switch (medium_loopCounter){

		// This case deals with the GPS
		//-------------------------------
		case 0:
			medium_loopCounter++;

			if(GPS_enabled){
				update_GPS();
			}
			break;

		// This case performs some navigation computations
		//------------------------------------------------
		case 1:
			medium_loopCounter++;

			if(g_gps->new_data){
				g_gps->new_data 	= false;

				// update navigation timer
				// -----------------------
				dTnav 				= DIYmillis() - medium_loopTimer;
				medium_loopTimer 	= DIYmillis();

				// calculate the plane's desired bearing
				// -------------------------------------
				navigate();

				// control mode specific updates to nav_bearing
				// --------------------------------------------
				update_navigation();
			}
			break;

		// unused
		//------------------------------
		case 2:
			medium_loopCounter++;

			break;

		// This case deals with sending high rate telemetry
		//-------------------------------------------------
		case 3:
			medium_loopCounter++;
			send_message(MSG_ATTITUDE);
			break;

		// This case controls the slow loop
		//---------------------------------
		case 4:
			medium_loopCounter=0;
			mediumLoop_event();
			slow_loop();
			break;
	}
}

void slow_loop()
{
	// This is the slow (3 1/3 Hz) loop pieces
	//----------------------------------------
	switch (slow_loopCounter){
		case 0:
			slow_loopCounter++;
			// nothing here
			break;

		case 1:
			slow_loopCounter++;

			// Read 3-position switch on radio
			// -------------------------------
			read_control_switch();

			// Read main battery voltage if hooked up - does not read the 5v from radio
			// ------------------------------------------------------------------------
			#if BATTERY_EVENT == 1
				read_battery();
			#endif

			break;

		case 2:
			slow_loopCounter = 0;
			// Reserved
			// Probably put uplink here
			break;
	}
}


void update_GPS(void)
{
	g_gps->update();

	// Blink GPS LED if we don't have a fix
	// ------------------------------------
	update_GPS_light();

	//current_loc.lng =   377697000;		// Lon * 10 * *7
	//current_loc.lat = -1224318000;		// Lat * 10 * *7
	//current_loc.alt = 100;				// alt * 10 * *7
	//return;

    if (g_gps->new_data && g_gps->fix) {

		if(ground_start_count > 1){
			ground_start_count--;

		} else if (ground_start_count == 1) {

			// We countdown N number of good GPS fixes
			// so that the altitude is more accurate
			// -------------------------------------
			if (current_loc.lat == 0) {
                //SendDebugln("!! bad loc");
				ground_start_count = 5;

			}else{
				init_home();
				ground_start_count = 0;
			}
		}

		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
		current_loc.lat = g_gps->latitude;		// Lat * 10 * *7
		current_loc.alt = g_gps->altitude;		// Lat * 10 * *7
	}
}

void update_current_flight_mode(void)
{
	switch(control_mode){
		case RTL:
		case LOITER:
		case AUTO:
			hold_course = -1;

			crash_checker();
			calc_nav_roll();
			calc_nav_pitch();
			calc_nav_throttle();
			break;

		case TAKEOFF:
			calc_nav_roll();
			//nav_pitch = (g_gps->ground_speed * TAKE_OFF_PITCH * 100) / 600;
			//nav_pitch = constrain(nav_pitch, 0, TAKE_OFF_PITCH * 100);

			float scaler;
			#if AIRSPEED_SENSOR == 1
				scaler = airspeed / airspeed_cruise;
				scaler = constrain(scaler,0,2);
				nav_pitch = scaler * TAKE_OFF_PITCH * 50;
			#else
				scaler = g_gps->ground_speed / airspeed_cruise;
				scaler = constrain(scaler,0,2);
				nav_pitch = scaler * TAKE_OFF_PITCH * 50;
			#endif

			nav_pitch = constrain(nav_pitch, 0, TAKE_OFF_PITCH * 100);

			// override pitch_sensor to limit porpoising
			//pitch_sensor 	= constrain(pitch_sensor, -6000, TAKE_OFF_PITCH * 100);
			// throttle = passthrough
			break;

		case LAND:
			// recalc bearing error
			//calc_bearing_error();
			calc_nav_roll();				// if we are closer than 20 m we hold our heading
			calc_nav_pitch();
			calc_nav_throttle();				// apply desired pitch to throttle for no airspeed option

			if (wp_distance > 0){

				if (land_complete) {
					rc_3.servo_out = 0;
					airspeed_cruise = 0;
					nav_pitch = LAND_PITCH * 100;	// override pitch for landing set by commands

				}else if (wp_distance <= wp_radius) {
					land_complete = 1;

				}else if(wp_distance < THROTTLE_CUT_RADIUS){
					rc_3.servo_out = 0;			// cut throttle to 0
					airspeed_cruise = AIRSPEED_SLOW * 100;

				}else if(wp_distance < SLOW_RADIUS){
					rc_3.servo_out = THROTTLE_SLOW;
					airspeed_cruise = AIRSPEED_SLOW * 100;
				}
			}
			break;

		case FLY_BY_WIRE_A:
			// fake Navigation output using sticks
			nav_roll 	= rc_1.control_in;
			nav_pitch 	= rc_2.control_in;

			rc_1.control_in = 0;
			rc_2.control_in = 0;

			nav_roll = constrain(nav_roll, -HEAD_MAX, HEAD_MAX);
			nav_pitch = constrain(nav_pitch, -3000, 3000);	// trying to give more pitch authority

			break;

		case FLY_BY_WIRE_B:
			// fake Navigation output using sticks
			// We use PITCH_MIN because its magnitude is normally greater than PITCH_MAX
			nav_roll 		= rc_1.control_in;
			nav_pitch 		= 0;
			altitude_error 	= rc_2.control_in;

			rc_1.control_in = 0;
			rc_2.control_in = 0;

			nav_roll = constrain(nav_roll, -HEAD_MAX, HEAD_MAX);

			#if AIRSPEED_SENSOR == 1
				airspeed_error = ((AIRSPEED_FBW_MAX - AIRSPEED_FBW_MIN) * rc_3.servo_out) + (AIRSPEED_FBW_MIN * 100);
				airspeed_energy_error = (long)(((long)airspeed_error * (long)airspeed_error) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
				airspeed_error = (airspeed_error - airspeed);
			#endif

			calc_nav_throttle();
			calc_nav_pitch();
			break;


		case CONFIG:
			// for better TE calcs
			airpseed_test += (int)airspeed;								// Small filtering
			airpseed_test = airpseed_test >> 1;
			throttle_test + throttle_test + rc_3.servo_out;		// Small filtering
			throttle_test = throttle_test >> 1;							// Small filtering
			airspeed_flag = true;
			// passthrough

		case STABILIZE:
			nav_roll 		= 0;
			nav_pitch 		= 0;

			// throttle is passthrough
			break;

		case CIRCLE:
			// we have no GPS installed and have lost radio contact
			// or we just want to fly around in a gentle circle w/o GPS
			// ----------------------------------------------------
			nav_roll = HEAD_MAX / 3;
			nav_pitch 		= 0;

			if (failsafe == true){
				rc_3.servo_out = THROTTLE_CRUISE;
			}
			break;

		case MANUAL:
			// servo_out is for Sim control only
			// ---------------------------------
			rc_1.servo_out  = rc_1.control_in;
			rc_2.servo_out  = rc_2.control_in;
			#if CH4_RUDDER == 1
				rc_4.servo_out  = rc_4.control_in;
			#endif

			if(airspeed_flag == true){
				airspeed_flag = false;
				eeprom_write_word((uint16_t *)	EE_AS_TEST,  airpseed_test);	// airpressure_offset
				eeprom_write_word((uint16_t *)	EE_THR_TEST,  throttle_test);	// airpressure_offset
			}

			break;
	}

	// If we have lost gps and have no ability to navigate we will circle at a gentle bank angle
	// This is another form of failsafe, different from losing radio signal.
	if(g_gps->fix == 0 && control_mode > FLY_BY_WIRE_B){
		// disabled until further testing
		//nav_roll = HEAD_MAX / 3;
		//nav_pitch = 0;
	}
}

void update_navigation()
{
	// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
	// ------------------------------------------------------------------------
	switch(control_mode){
		case RTL:
		case LOITER:
			float power;
			if (wp_distance <= loiter_radius){
				nav_bearing -= 9000;

			}else if (wp_distance < (loiter_radius + LOITER_RANGE)){
				power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE);
				power = constrain(power, 0, 1);
				nav_bearing -= power * 9000;

			}else{
				update_crosstrack();
			}
			break;

		case LAND:
			if ((wp_distance > 0) && (wp_distance <= 30)) {
				land_complete = 1;
				if(hold_course == -1){
					// save our current course to land
					hold_course = g_gps->ground_course;
				}
			}
			if(hold_course > -1){
				// recalc bearing error with hold_course;
				nav_bearing = hold_course;
				// recalc bearing error
				calc_bearing_error();
			}
			update_crosstrack();
			break;

		case AUTO:
			// reset hold course
			hold_course == -1;

			// check if we have missed the WP
			loiter_delta = (target_bearing - old_target_bearing)/100;

			// reset the old value
			old_target_bearing = target_bearing;

			// wrap values
			if (loiter_delta > 170) loiter_delta -= 360;
			if (loiter_delta < -170) loiter_delta += 360;
			loiter_sum += loiter_delta;

			if (wp_distance <= 0){
				// do nothing

			}else if (wp_distance <= wp_radius){
				waypoint_event(EVENT_WILL_REACH_WAYPOINT);
				reached_waypoint();

			}else{
				update_crosstrack();
				if(loiter_sum > 300){
					//Serial.println("MSG missed WP");
					waypoint_event(EVENT_WILL_REACH_WAYPOINT);
					reached_waypoint();
				}else if (loiter_sum < -300){
					//Serial.println("MSG missed WP");
					waypoint_event(EVENT_WILL_REACH_WAYPOINT);
					reached_waypoint();
				}
			}
			break;

		case TAKEOFF:
			if (g_gps->ground_speed > 3){
				if(hold_course == -1){
					// save our current course to land
					hold_course = g_gps->ground_course;
				}
			}
			if(hold_course > -1){
				// recalc bearing error with hold_course;
				nav_bearing = hold_course;
			}
			if (current_loc.alt > (home.alt + (long)TAKE_OFF_ALT * 100))  {
				takeoff_complete = true;
				hold_course = -1;
				reset_control_switch();
			}
			break;
	}
}